import os
import math
from launch import LaunchDescription
from launch_ros.actions import ComposableNodeContainer, Node
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    # -------------------------- 参数文件路径 --------------------------
    # 相机节点参数
    mindvision_pkg = get_package_share_directory('mindvision_camera')
    camera_params_file = os.path.join(mindvision_pkg, 'config', 'camera_params.yaml')
    camera_info_url = 'package://mindvision_camera/config/camera_info.yaml'

    # 视觉识别节点参数
    hero_vision_pkg = get_package_share_directory('hero_integrated_vision')
    hero_vision_params_file = os.path.join(hero_vision_pkg, 'config', 'hero.yaml')

    # 追踪节点参数
    armor_tracker_pkg = get_package_share_directory('armor_tracker')
    tracker_params_file = os.path.join(armor_tracker_pkg, 'config', 'armor_tracker.yml')

    # （可选）调试：打印路径，确认是否正确
    print("参数文件路径：", tracker_params_file)

    # -------------------------- 组件容器（核心：整合三个组件） --------------------------
    container = ComposableNodeContainer(
        name='hero_aim_container',  # 容器名称，可自定义
        namespace='',
        package='rclcpp_components',  # 固定容器包名
        executable='component_container',  # 固定容器可执行文件
        output='screen',  # 日志输出到终端
        composable_node_descriptions=[
            # 1. 相机组件（mindvision_camera）
            ComposableNode(
                package='mindvision_camera',
                plugin='mindvision_camera::MVCameraNode',  # 组件类名（必须与代码注册宏一致）
                name='mindvision_camera_node',
                parameters=[
                    camera_params_file,  # 从yaml读取所有相机参数（含use_sensor_data_qos）
                    {'camera_info_url': camera_info_url}  # 单独配置URL参数
                ]
            ),

            # 2. 视觉识别组件（hero_integrated_vision）
            ComposableNode(
                package='hero_integrated_vision',
                plugin='rm_aim::HeroVisionNode',  # 组件类名（需与代码注册宏一致）
                name='hero_vision_node',
                parameters=[hero_vision_params_file]  # 加载原yaml参数
            ),

            # 3. 装甲板追踪组件（armor_tracker）
            ComposableNode(
                package='armor_tracker',
                plugin='rm_aim::ArmorTrackerNode',  # 已注册的组件类名
                name='armor_tracker_node',
                parameters=[tracker_params_file]  # 加载原yaml参数
            )
        ]
    )

    # -------------------------- 静态TF节点（保持独立启动） --------------------------
    static_tf_node = Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name='static_tf_pub',
        arguments=[
            '0', '0', '0',  # x,y,z 平移量（根据实际安装位置调整）
            str(-math.pi/2), '0', str(-math.pi/2),  # 滚转,俯仰,偏航（相机光学帧到odom的转换）
            'odom', 'camera_optical_frame'  # 父坐标系->子坐标系
        ]
    )

    # -------------------------- 组装Launch描述 --------------------------
    return LaunchDescription([
        container,  # 组件容器（包含三个核心节点）
        static_tf_node  # 静态TF变换发布器
    ])